//
// Created by HFauto on 25-7-31.
//

#include "zxMpu6500.h"

static const char* TAG = "mpu6500";
zxMpu6500::zxMpu6500() {
}

zxMpu6500::~zxMpu6500() {
}

void zxMpu6500::init(gpio_num_t sda, gpio_num_t scl, int addr) {
    i2c_sda  = sda;
    i2c_scl  = scl;
    i2c_addr = addr;
    ESP_ERROR_CHECK(i2cdev_init());
    ESP_ERROR_CHECK(mpu6050_init_desc(&dev, i2c_addr, 0, i2c_sda, i2c_scl));

    while (true){
        esp_err_t res = i2c_dev_probe(&dev.i2c_dev, I2C_DEV_WRITE);
        if (res == ESP_OK)
        {
            ESP_LOGI(TAG, "Found MPU60x0 device");
            break;
        }
        ESP_LOGE(TAG, "MPU60x0 not found");
        vTaskDelay(pdMS_TO_TICKS(1000));
    }

    ESP_ERROR_CHECK(mpu6050_init(&dev));
    ESP_LOGI(TAG, "Accel range: %d", dev.ranges.accel);
    ESP_LOGI(TAG, "Gyro range:  %d", dev.ranges.gyro);
}

bool zxMpu6500::runTest() {
    return false;
}

void zxMpu6500::taskFunction() {
        float temp;
        ESP_ERROR_CHECK(mpu6050_get_temperature(&dev, &temp));
        ESP_ERROR_CHECK(mpu6050_get_motion(&dev, &accel, &rotation));
        ESP_ERROR_CHECK(mpu6050_get_raw_motion(&dev, &accel_raw, &rotation_raw));
        // ESP_LOGI(TAG, "**********************************************************************");
        // ESP_LOGI(TAG, "Acceleration: x=%.4f   y=%.4f   z=%.4f", accel.x, accel.y, accel.z);
        // ESP_LOGI(TAG, "Rotation:     x=%.4f   y=%.4f   z=%.4f", rotation.x, rotation.y, rotation.z);
        // ESP_LOGI(TAG, "Temperature:  %.1f", temp);
}

